﻿module FMath

open UI
open System

//Vector2 definition
type Vector2<[<Measure>] 'a> (X : float<'a>, Y : float<'a>) = 
    let mutable _X = X
    let mutable _Y = Y

    member this.X = _X
    member this.Y = _Y

   
    //return a XNA compatible Vector2
    member this.xnaVector2 = Microsoft.Xna.Framework.Vector2(X |> float32, Y |> float32)

    //return a XNA compatible Vector3 with view from Y axe
    member this.xnaVector3YView = Microsoft.Xna.Framework.Vector3(X |> float32, 0 |> float32, Y |> float32)
    
    member this.Lenght : float<'a> = sqrt(this.X * this.X + this.Y * this.Y)
    member this.LenghtSquared : float<'a * 'a> = this.X * this.X + this.Y * this.Y
    member this.Normalize : Vector2<1> = Vector2(this.X / this.Lenght, this.Y / this.Lenght)

    //Direction Vectors
    static member Up = Vector2(0.0<1>,-1.0<1>);
    static member Down = Vector2(0.0<1>,1.0<1>);
    static member Left = Vector2(-1.0<1>,0.0<1>);
    static member Right = Vector2(1.0<1>,0.0<1>);

    //Check for equality between Two Vectors of the same type
    static member ( == ) (a:Vector2<'a>, b:Vector2<'a>) : bool =
     if(a.X = b.X) && (a.Y = b.Y) then
      true
     else
      false

    static member ( != ) (a:Vector2<'a>, b:Vector2<'a>) : bool =
     if(a == b) then
      false
     else
      true

    //Vector negation
    static member ( ~- ) (a:Vector2<'a>) : Vector2<'a> = Vector2<'a>(-a.X, -a.Y)

    //Vector sum
    static member ( + ) (a:Vector2<'a>, b:Vector2<'a>) : Vector2<'a> = Vector2(a.X + b.X, a.Y + b.Y)

    //Vector difference
    static member ( - ) (a:Vector2<'a>, b:Vector2<'a>) : Vector2<'a> = a + (-b)

    //Vector dot scalar
    static member ( * ) (a:Vector2<'a>, b:float<'b>) : Vector2<'a * 'b> = Vector2(a.X * b, a.Y * b)
    static member ( * ) (b:float<'b>, a:Vector2<'a>) : Vector2<'b * 'a> = Vector2(a.X * b, a.Y * b)

    //Distance between two points
    static member Distance(a:Vector2<'a>,b:Vector2<'a>) : float<'a> = 
     let v = a - b
     v.Lenght

 type Side =
   | Top = 0
   | Bottom = 1
   | Left = 2
   | Right = 3

//Square definition
type Square (center: Vector2<m>, size: float<m>) =
  let mutable center = center
  let size = size
  let offset = size * 0.5
  let mutable upLeft = Vector2<m>(center.X - offset, center.Y - offset)
  let mutable upRight = Vector2<m>(center.X + offset, center.Y - offset)
  let mutable downLeft = Vector2<m>(center.X - offset, center.Y + offset)
  let mutable downRight = Vector2<m>(center.X + offset, center.Y + offset)

  member this.Size = size

  member this.Center 
   with set(newCenter) = 
    center <- newCenter
    upLeft <- Vector2<m>(center.X - offset, center.Y - offset)
    upRight <- Vector2<m>(center.X + offset, center.Y - offset)
    downLeft <- Vector2<m>(center.X - offset, center.Y + offset)
    downRight <- Vector2<m>(center.X + offset, center.Y + offset)
   and get() = center

  member this.UpLeft = upLeft;
  member this.UpRight = upRight;
  member this.DownLeft = downLeft;
  member this.DownRight = downRight;

  member this.Intersect (otherSquare:Square, side: Side) = 
    let mutable relativeIntersectionStart = 0.0
    let mutable relativeIntersectionEnd = 0.0
    match side with
     | Side.Top ->  
        if ((otherSquare.DownLeft.Y >= this.UpLeft.Y) && (otherSquare.UpLeft.Y <= this.UpLeft.Y)) ||
           ((otherSquare.DownLeft.Y <= this.DownLeft.Y) && (otherSquare.UpLeft.Y >= this.UpLeft.Y)) then
            relativeIntersectionStart <- (otherSquare.DownLeft.X - this.UpLeft.X) / this.Size
            relativeIntersectionEnd <- (otherSquare.DownRight.X - this.UpLeft.X) / this.Size
     | Side.Bottom -> 
        if ((otherSquare.UpLeft.Y <= this.DownLeft.Y) && (otherSquare.DownLeft.Y >= this.UpLeft.Y)) ||
           ((otherSquare.UpLeft.Y >= this.UpLeft.Y) && (otherSquare.DownLeft.Y <= this.DownLeft.Y)) then
            relativeIntersectionStart <- (otherSquare.UpLeft.X - this.DownLeft.X) / this.Size
            relativeIntersectionEnd <- (otherSquare.UpRight.X - this.DownLeft.X) / this.Size
     | Side.Left -> 
        if ((otherSquare.DownRight.X >= this.DownLeft.X) && (otherSquare.DownLeft.X <= this.DownLeft.X)) ||
           ((otherSquare.DownRight.X <= this.DownRight.X) && (otherSquare.DownLeft.X >= this.DownLeft.X)) then
            relativeIntersectionStart <- (otherSquare.UpRight.Y - this.UpLeft.Y) / this.Size
            relativeIntersectionEnd <- (otherSquare.DownRight.Y - this.UpLeft.Y) / this.Size
     | Side.Right -> 
        if ((otherSquare.DownLeft.X <= this.DownRight.X) && (otherSquare.DownRight.X >= this.DownRight.X)) ||
           ((otherSquare.DownLeft.X >= this.DownLeft.X) && (otherSquare.DownRight.X <= this.DownRight.X)) then
            relativeIntersectionStart <- (otherSquare.UpLeft.Y - this.UpRight.Y) / this.Size
            relativeIntersectionEnd <- (otherSquare.DownLeft.Y - this.UpRight.Y) / this.Size
     | _ -> ()  
    
    if (relativeIntersectionEnd > 1.0) then 
     relativeIntersectionEnd <- 1.0

    if (relativeIntersectionStart < 0.0) then 
     relativeIntersectionStart <- 0.0
    
    if (relativeIntersectionEnd < 0.0) || (relativeIntersectionStart > 1.0) then 
     (0.0,0.0)
    else
     (relativeIntersectionStart, relativeIntersectionEnd)

 let newWheelRollTheta (newPosition: Vector2<m>, oldPosition: Vector2<m>, wheelRadius : float<m>, oldWheelRollTheta) = 
  let theta = (newPosition - oldPosition).Lenght / wheelRadius + oldWheelRollTheta
  if theta > 6.28 then 0.0
  else theta
